/*
 * arm5e_ik.cpp
 *
 *  Created on: 28/09/2012
 *      Author: mprats
 */

#include <ros/ros.h>
#include <mar_robot_arm5e/ARM5Arm.h>

#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char **argv) {
	ros::init(argc, argv,"arm5e_ik");
	ros::NodeHandle nh;

	ARM5Arm arm;


	//Publish tf of auv pose
	tf::TransformBroadcaster br;

	ros::Publisher js_pub=nh.advertise<sensor_msgs::JointState>("joint_states", 1);
	ros::Publisher pose_pub=nh.advertise<geometry_msgs::Pose>("/g500/pose", 1);

	ros::Rate r(5);
	while (ros::ok()) {
		ros::spinOnce();

		double goalx, goaly, goalz, goalrx, goalry, goalrz;
		nh.getParam("goalx", goalx);
		nh.getParam("goaly", goaly);
		nh.getParam("goalz", goalz);
		nh.getParam("goalrx", goalrx);
		nh.getParam("goalry", goalry);
		nh.getParam("goalrz", goalrz);

		vpHomogeneousMatrix wMe(goalx, goaly, goalz, goalrx, goalry ,goalrz);
		vpColVector q;
		q=arm.vehicleArmIK(wMe);

		//Publish joint states for visualization in rviz
		tf::Transform transform, transform_goal;
		transform.setIdentity();
		transform.setOrigin(tf::Vector3(q[0], q[1], q[2]));
		transform.setRotation(tf::Quaternion(tf::Vector3(0,0,1), q[3]));
		transform_goal.setOrigin(tf::Vector3(wMe[0][3], wMe[1][3], wMe[2][3]));
		transform_goal.setBasis(btMatrix3x3(wMe[0][0], wMe[0][1], wMe[0][2],
				wMe[1][0], wMe[1][1], wMe[1][2],
				wMe[2][0], wMe[2][1], wMe[2][2]));
		br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), std::string("world"), std::string("base_link")));
		br.sendTransform(tf::StampedTransform(transform_goal, ros::Time::now(), std::string("world"), std::string("goal")));

		sensor_msgs::JointState jmsg;
		jmsg.header.stamp=ros::Time::now();
		jmsg.name.push_back("part0_to_part1");
		jmsg.position.push_back(q[4]);
		jmsg.name.push_back("part1_to_part2");
		jmsg.position.push_back(q[5]);
		jmsg.name.push_back("part2_to_part3");
		jmsg.position.push_back(q[6]);
		jmsg.name.push_back("part3_to_part4_base");
		jmsg.position.push_back(q[7]);
		//Publish the rest of joints to 0
		jmsg.name.push_back("part4_to_part4_jaw1");
		jmsg.name.push_back("part4_to_part4_jaw2");
		jmsg.position.push_back(0);
		jmsg.position.push_back(0);

		js_pub.publish(jmsg);

		//Publish pose and joint states to UWSim
		geometry_msgs::Pose pose_msg;
		tf::poseTFToMsg(transform, pose_msg);
		pose_pub.publish(pose_msg);

		r.sleep();
	}
}

